#include "hdmap_layer_api.h"
#include "AutoSDK.hpp"
#include "hdmap/IHDMap.hpp"
#include "hdmap/Road.hpp"
#include "base/defines.h"
#include "base/coordRef.h"
#include "base/common.h"
#include "base/log.h"
#include <eigen3/Eigen/Core>
#include <iostream>
#include <vector>

using namespace MAPSDK;

namespace MAPSDK
{
    class HdMapLayerImpl
    {
    public:
        HdMapLayerImpl()
        {
            autosdk_ = CreateAutoSDK();
        }

        ~HdMapLayerImpl()
        {
            autosdk_->Release();
            autosdk_ = nullptr;
        }

        bool MapInit(const std::string &map_path)
        {
            if (!autosdk_->Config(map_path))
            {
                std::cout << "autosdk configuration failed!" << std::endl;
                return false;
            }
            if (!autosdk_->Start())
            {
                std::cout << "autosdk start failed!" << std::endl;
                return false;
            }

            map_handler_ = autosdk_->GetHDMap();
            if (!map_handler_)
            {
                std::cout << "fail to get hdmap handler!" << std::endl;
                return false;
            }
            return true;
        }

        std::vector<hdmap_sign> SearchSignsByRadius(const GeoPoint &gps_point, double radius)
        {
            Navinfo::Engine::Pos3D loc;
            loc.m_Longitude = gps_point.longitude;
            loc.m_Latitude = gps_point.latitude;
            loc.m_Altitude = gps_point.altitude;
            map_handler_->SetLocation(&loc);

            std::vector<uint64_t> sign_ids;
            Navinfo::Common::ReturnCode res = map_handler_->SearchObjectsByRadiusRange(&loc, radius, Navinfo::Common::ENObjectType::OBJECT_TYPE_SIGN, sign_ids);
            if (res != Navinfo::Common::ReturnCode::Code_Success || sign_ids.empty())
            {
                return std::vector<hdmap_sign>();
            }

            std::vector<hdmap_sign> hdmap_signs;
            for (uint64_t id : sign_ids)
            {
                Navinfo::HDMap::Object oObject;
                map_handler_->GetObjectByID(id, Navinfo::Common::ENObjectType::OBJECT_TYPE_SIGN, oObject);
                std::vector<std::vector<Navinfo::Engine::Math::Vec3d>> geometry_points = oObject.GetGeometryPoints();
                assert(geometry_points.size() > 0);

                hdmap_sign sign;
                for (const Navinfo::Engine::Math::Vec3d &v : geometry_points[0])
                {
                    double x, y, z;
                    CoordRef::Instance().WGS2GKP(v.x(), v.y(), v.z(), x, y, z);
                    sign.geometry.emplace_back(x, y, z);
                }
                sign.geometry.emplace_back(sign.geometry[0]);
                hdmap_signs.push_back(sign);
            }
            return hdmap_signs;
        }

        std::vector<hdmap_lane> SearchLanesByRadius(const GeoPoint &gps_point, double radius)
        {
            Navinfo::Engine::Pos3D loc;
            loc.m_Longitude = gps_point.longitude;
            loc.m_Latitude = gps_point.latitude;
            loc.m_Altitude = gps_point.altitude;
            std::cout << "longitude :" <<  gps_point.longitude;
            std::cout << "latitude : " << gps_point.latitude;
            std::cout << "altitude : " << gps_point.altitude;
            map_handler_->SetLocation(&loc);

            std::vector<uint64_t> lane_boundarys;
            Navinfo::Common::ReturnCode res = map_handler_->SearchLaneBoundaryGroupsByRadiusRange(&loc, radius, lane_boundarys);
            if (res != Navinfo::Common::ReturnCode::Code_Success || lane_boundarys.empty())
            {
                return std::vector<hdmap_lane>();
            }

            std::vector<hdmap_lane> hdmap_lanes;
            for (uint64_t id : lane_boundarys)
            {
                Navinfo::HDMap::LaneBoundaryGroup lbg;
                map_handler_->GetLaneBoundaryGroupByID(id, lbg);
                const std::vector<std::shared_ptr<Navinfo::Engine::HDMapLaneBoundaryInfo>> &lbs = lbg.GetLaneBoundaries();
                for (const auto &lb : lbs)
                {
                    const std::vector<std::shared_ptr<Navinfo::Engine::HDMapLaneBoundaryElementInfo>> &lb_infos = lb->GetLaneBoundaryElements();
                    int mark_type = 0;
                    for (const auto &lb_info : lb_infos)
                    {
                        Navinfo::Engine::HDMapLaneBoundaryType lane_boundary_type = lb_info->GetLaneBoundaryType();
                        if (lane_boundary_type == Navinfo::Engine::HDMapLaneBoundaryType::LONG_DASHED_LINE)
                        {
                            mark_type = 2;
                            break;
                        }
                        else if (lane_boundary_type == Navinfo::Engine::HDMapLaneBoundaryType::SINGLE_SOLID_LINE)
                        {
                            mark_type = 1;
                            break;
                        }
                        else if (lane_boundary_type == Navinfo::Engine::HDMapLaneBoundaryType::DOUBLE_SOLID_LINE
                          || lane_boundary_type == Navinfo::Engine::HDMapLaneBoundaryType::SOLID_LINE_DASHED_LINE
                          || lane_boundary_type == Navinfo::Engine::HDMapLaneBoundaryType::DASHED_LINE_SOLID_LINE 
                          || lane_boundary_type == Navinfo::Engine::HDMapLaneBoundaryType::SHORT_DASHED_LINE 
                          || lane_boundary_type == Navinfo::Engine::HDMapLaneBoundaryType::DOUBLE_SOLID_LINE 
                          || lane_boundary_type == Navinfo::Engine::HDMapLaneBoundaryType::DOUBLE_DASHED_LINE)
                        {
                            mark_type = 4;
                            break;
                        }
                    }
                    if (mark_type == 0)
                    {
                        continue;
                    }

                    std::vector<Eigen::Vector3d> geometry;
                    const std::vector<Navinfo::Engine::Math::Vec3d> &geometry_points = lb->GetGeometryPoints();
                    for (const Navinfo::Engine::Math::Vec3d &v : geometry_points)
                    {
                        double x, y, z;
                        CoordRef::Instance().WGS2GKP(v.x(), v.y(), v.z(), x, y, z);
                        geometry.emplace_back(x, y, z);
                    }

                    hdmap_lane lane;
                    lane.id = id;
                    lane.geometry = geometry;
                    lane.mark_type = mark_type;
                    hdmap_lanes.push_back(std::move(lane));
                }
            }
            return hdmap_lanes;
        }

    private:
        Navinfo::Engine::AutoSDK *autosdk_;
        Navinfo::Engine::IHDMap *map_handler_;
    };

} // namespace AUTOSDK

HDMapLayer::HDMapLayer()
{
    map_layer_impl_ = new HdMapLayerImpl;
    map_layer_impl_->MapInit("../config/config.ini");
}

HDMapLayer::~HDMapLayer()
{
    delete map_layer_impl_;
    map_layer_impl_ = nullptr;
}

 std::vector<hdmap_lane> HDMapLayer::SearchLanesByRadius(const GeoPoint &gps_point, double radius)
 {
     return map_layer_impl_->SearchLanesByRadius(gps_point, radius);
 }

 std::vector<hdmap_sign> HDMapLayer::SearchSignsByRadius(const GeoPoint &gps_point, double radius)
 {
     return map_layer_impl_->SearchSignsByRadius(gps_point, radius);
 }

  hdmap_frame HDMapLayer::SearchByRadius(const GeoPoint &gps_point, double radius)
  {
    hdmap_frame hdmap_f;
    hdmap_f.hdmap_lanes = SearchLanesByRadius(gps_point, radius);
    hdmap_f.hdmap_signs = SearchSignsByRadius(gps_point, radius * 10);
    return hdmap_f;
  }